#include "camControl/algorithmImage.hpp"

using namespace cv;

AlgorithmImage::AlgorithmImage(unsigned int width, unsigned int height) {

    this->width = width;
    this->height = height;

    __cyan.min = MIN_CYAN;
    __cyan.max = MAX_CYAN;
    __cyan.typeColor = CYAN__;
    __cyan.rgb.B = 255;
    __cyan.rgb.G = 255;
    __cyan.rgb.R = 0;

    __green.min = MIN_GREEN;
    __green.max = MAX_GREEN;
    __green.typeColor = GREEN__;
    __green.rgb.B = 0;
    __green.rgb.G = 255;
    __green.rgb.R = 0;

    __orange.min = MIN_ORANGE;
    __orange.max = MAX_ORANGE;
    __orange.typeColor = ORANGE__;
    __orange.rgb.B = 0;
    __orange.rgb.G = 255;
    __orange.rgb.R = 255;


    __pink.min = MIN_PINK;
    __pink.max = MAX_PINK;
    __pink.typeColor = PINK__;
    __pink.rgb.B = 0;
    __pink.rgb.G = 0;
    __pink.rgb.R = 255;

    colorImg = NULL;

    poly1_hull0 = 0;
    perimScale = 100;
    numRect = NUM_MAX_BLOBS;

    image = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
    hsv = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);

#ifdef WORK_CYAN

    tempCyan = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
    segCyan = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);
    segImgCyan = NULL;
    tempImgCyan = NULL;

#endif

#ifdef WORK_GREEN

    tempGreen = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
    segGreen = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);
    segImgGreen = NULL;
    tempImgGreen = NULL;

#endif

#ifdef WORK_ORANGE

    tempOrange = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
    segOrange = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);
    segImgOrange = NULL;
    tempImgOrange = NULL;

#endif

#ifdef WORK_PINK

    tempPink = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
    segPink = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);
    segImgPink = NULL;
    tempImgPink = NULL;

#endif
}

AlgorithmImage::~AlgorithmImage() {

    cvReleaseImageHeader(&image);
    cvReleaseImage(&hsv);

#ifdef WORK_CYAN

    cvReleaseImageHeader(&tempCyan);
    cvReleaseImage(&segCyan);

#endif

#ifdef WORK_GREEN

    cvReleaseImageHeader(&tempGreen);
    cvReleaseImage(&segGreen);

#endif

#ifdef WORK_ORANGE

    cvReleaseImageHeader(&tempOrange);
    cvReleaseImage(&segOrange);

#endif

#ifdef WORK_PINK

    cvReleaseImageHeader(&tempPink);
    cvReleaseImage(&segPink);

#endif
}

void AlgorithmImage::segmentedByColors() {

    for (int y = 0; y < image->height; y++) {

        colorImg = (uchar*) (hsv->imageData + y * hsv->widthStep);

#ifdef WORK_CYAN
        segImgCyan = (uchar*) (segCyan->imageData + y * segCyan->widthStep);
        tempImgCyan = (uchar*) (tempCyan->imageData + y * tempCyan->widthStep);
#endif

#ifdef WORK_GREEN            
        segImgGreen = (uchar*) (segGreen->imageData + y * segGreen->widthStep);
        tempImgGreen = (uchar*) (tempGreen->imageData + y * tempGreen->widthStep);
#endif

#ifdef WORK_ORANGE
        segImgOrange = (uchar*) (segOrange->imageData + y * segOrange->widthStep);
        tempImgOrange = (uchar*) (tempOrange->imageData + y * tempOrange->widthStep);
#endif

#ifdef WORK_PINK
        segImgPink = (uchar*) (segPink->imageData + y * segPink->widthStep);
        tempImgPink = (uchar*) (tempPink->imageData + y * tempPink->widthStep);
#endif

        for (int x = 0; x < image->width; x++) {

#ifdef WORK_CYAN
            segImgCyan[x] = 0;
            tempImgCyan[3 * x] = 0;
            tempImgCyan[3 * x + 1] = 0;
            tempImgCyan[3 * x + 2] = 0;

            if ((colorImg[3 * x] >= __cyan.min) && (colorImg[3 * x] <= __cyan.max)) {
                segImgCyan[x] = __cyan.typeColor;
                tempImgCyan[3 * x] = __cyan.rgb.B;
                tempImgCyan[3 * x + 1] = __cyan.rgb.G;
                tempImgCyan[3 * x + 2] = __cyan.rgb.R;
            }

#endif

#ifdef WORK_GREEN
            segImgGreen[x] = 0;
            tempImgGreen[3 * x] = 0;
            tempImgGreen[3 * x + 1] = 0;
            tempImgGreen[3 * x + 2] = 0;

            if ((colorImg[3 * x] >= __green.min) && (colorImg[3 * x] <= __green.max)) {
                segImgGreen[x] = __green.typeColor;
                tempImgGreen[3 * x] = __green.rgb.B;
                tempImgGreen[3 * x + 1] = __green.rgb.G;
                tempImgGreen[3 * x + 2] = __green.rgb.R;
            }
#endif

#ifdef WORK_ORANGE
            segImgOrange[x] = 0;
            tempImgOrange[3 * x] = 0;
            tempImgOrange[3 * x + 1] = 0;
            tempImgOrange[3 * x + 2] = 0;

            if ((colorImg[3 * x] >= __orange.min) && (colorImg[3 * x] <= __orange.max)) {
                segImgOrange[x] = __orange.typeColor;
                tempImgOrange[3 * x] = __orange.rgb.B;
                tempImgOrange[3 * x + 1] = __orange.rgb.G;
                tempImgOrange[3 * x + 2] = __orange.rgb.R;
            }

#endif

#ifdef WORK_PINK
            segImgPink[x] = 0;
            tempImgPink[3 * x] = 0;
            tempImgPink[3 * x + 1] = 0;
            tempImgPink[3 * x + 2] = 0;

            if ((colorImg[3 * x] >= __pink.min) && (colorImg[3 * x] <= __pink.max)) {
                segImgPink[x] = __pink.typeColor;
                tempImgPink[3 * x] = __pink.rgb.B;
                tempImgPink[3 * x + 1] = __pink.rgb.G;
                tempImgPink[3 * x + 2] = __pink.rgb.R;
            }
#endif
        }
    }
}

void AlgorithmImage::segmentedImage() {

    try {

        if (!image || !hsv) throw "image or hsv NULL";

#ifdef PARALLEL              

        cvCvtColor(image, hsv, CV_BGR2HSV);
        int size = image->imageSize;
        static uchar *cyan_ = (uchar *) malloc(size);
        static uchar *green_ = (uchar *) malloc(size);
        static uchar *orange_ = (uchar *) malloc(size);
        static uchar *pink_ = (uchar *) malloc(size);

#ifdef WORK_CYAN
        memcpy(cyan_, image->imageData, size);
        cvSetData(tempCyan, cyan_, tempCyan->widthStep);
#endif

#ifdef WORK_GREEN
        memcpy(green_, image->imageData, size);
        cvSetData(tempGreen, green_, tempGreen->widthStep);
#endif

#ifdef WORK_ORANGE
        memcpy(orange_, image->imageData, size);
        cvSetData(tempOrange, orange_, tempOrange->widthStep);
#endif

#ifdef WORK_PINK
        memcpy(pink_, image->imageData, size);
        cvSetData(tempPink, pink_, tempPink->widthStep);
#endif
    
        segmentedByColors();
#endif
    } catch (char *error) {
        std::cout << error << std::endl;
    }
}

void AlgorithmImage::findCountorsByColor(IplImage *__temp, IplImage *__seg, CvScalar __scalar, int *__numColors, CvPoint *__centers) {

    static CvMemStorage* mem_storage = NULL;
    static CvSeq* contours = NULL;

    cvZero(__temp);

    cvMorphologyEx(__seg, __seg, 0, 0, CV_MOP_OPEN, CVCLOSE_ITR);
    cvMorphologyEx(__seg, __seg, 0, 0, CV_MOP_CLOSE, CVCLOSE_ITR);

    //FIND CONTOURS AROUND ONLY BIGGER REGIONS
    if (mem_storage == NULL) {
        mem_storage = cvCreateMemStorage(0);
    } else {
        cvClearMemStorage(mem_storage);
    }

    // cvStartFindContour only works for grayscale images
    CvContourScanner scanner = cvStartFindContours(__seg, mem_storage, sizeof (CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
    CvSeq* c;
    int numCont = 0;
    while ((c = cvFindNextContour(scanner)) != NULL) {
        double len = cvContourPerimeter(c);
        // calculate perimeter len threshold:

        double q = (__seg->height + __seg->width) / perimScale;
        //Get rid of blob if its perimeter is too small:

        if (len < q) {
            cvSubstituteContour(scanner, NULL);
        } else {
            // Smooth its edges if its large enough
            CvSeq* c_new;
            if (poly1_hull0) {

                //Polygonal approximation
                c_new = cvApproxPoly(c, sizeof (CvContour), mem_storage, CV_POLY_APPROX_DP, CVCONTOUR_APPROX_LEVEL, 0);
            } else {
                // Convex Hull of the segmentation
                c_new = cvConvexHull2(c, mem_storage, CV_CLOCKWISE, 1);
            }
            cvSubstituteContour(scanner, c_new);
            numCont++;
        }
    }
    contours = cvEndFindContours(&scanner);

    IplImage *mask = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
    cvZero(mask);

    static CvScalar CVX_WHITE = CV_RGB(0xff, 0xff, 0xff);
    CvMoments moments;
    double M00, M01, M10;

    int i = 0;
    c = contours;
    for (; c != NULL; i++, c = c->h_next) {

        cvDrawContours(mask, c, CVX_WHITE, CVX_WHITE, -1, CV_FILLED, 8);
        if (i < NUM_MAX_BLOBS) {
            cvMoments(mask, &moments, 1);
            M00 = cvGetSpatialMoment(&moments, 0, 0);
            M10 = cvGetSpatialMoment(&moments, 1, 0);
            M01 = cvGetSpatialMoment(&moments, 0, 1);
            __centers[i].x = (int) (M10 / M00);
            __centers[i].y = (int) (M01 / M00);
        }

        //Bounding rectangles around blobs
        if (rect != NULL)
            rect[i] = cvBoundingRect(c);

        //Draw filled contours into mask
        cvDrawContours(image, c, __scalar, __scalar, -1, CV_FILLED, 8);
    }

    if (mem_storage != NULL) {
        cvClearMemStorage(mem_storage);
    }

    cvReleaseImage(&mask);
    *__numColors = i;
}

void AlgorithmImage::findConnectedComponents() {

    try {

#ifdef PARALLEL 

        int seqCyan, seqGreen, seqOrange, seqPink;
        int num, i, j;
        static CvPoint centerCyan[NUM_MAX_BLOBS], centerGreen[NUM_MAX_BLOBS], centerOrange[NUM_MAX_BLOBS], centerPink[NUM_MAX_BLOBS];
        CvScalar scalar;
        num = i = j = 0;

#ifdef WORK_CYAN
        scalar = cvScalar(255, 255, 0);
       // findCountorsByColor(tempCyan, segCyan, scalar, &seqCyan, centerCyan);
        boost::thread cyanThread(&AlgorithmImage::findCountorsByColor,this,tempCyan, segCyan, scalar, &seqCyan,centerCyan); 
#endif

#ifdef WORK_GREEN
        scalar = cvScalar(0, 255, 0);
//        findCountorsByColor(tempGreen, segGreen, scalar, &seqGreen, centerGreen);
        boost::thread greenThread(&AlgorithmImage::findCountorsByColor,this,tempGreen, segGreen, scalar, &seqGreen,centerGreen); 
#endif

#ifdef WORK_ORANGE
        scalar = cvScalar(0, 255, 255);
    //  findCountorsByColor(tempOrange, segOrange, scalar, &seqOrange, centerOrange);
        boost::thread orangeThread(&AlgorithmImage::findCountorsByColor,this, tempOrange, segOrange, scalar, &seqOrange,centerOrange); 
#endif

#ifdef WORK_PINK
        scalar = cvScalar(0, 0, 255);
       // findCountorsByColor(tempPink, segPink, scalar, &seqPink, centerPink);
        boost::thread pinkThread(&AlgorithmImage::findCountorsByColor,this, tempPink, segPink, scalar, &seqPink,centerPink); 
       // num += seqPink;
#endif

#ifdef WORK_CYAN
    cyanThread.join();
#endif
#ifdef WORK_GREEN
    greenThread.join();
#endif
#ifdef WORK_ORANGE    
    orangeThread.join();
#endif
#ifdef WORK_PINK
    pinkThread.join();
 #endif

#ifdef WORK_CYAN
        num = seqCyan;
        for (j = 0; i < num; i++, j++) {
            this->centers[i] = centerCyan[j];
            this->colors[i] = CYAN__;
        }
#endif

#ifdef WORK_GREEN
        num += seqGreen;
        for (j = 0; i < num; i++, j++) {
            this->centers[i] = centerGreen[j];
            this->colors[i] = GREEN__;
        }
#endif

#ifdef WORK_ORANGE
        num += seqOrange;
        for (j = 0; i < num; i++, j++) {
            this->centers[i] = centerOrange[j];
            this->colors[i] = ORANGE__;
        }
#endif

#ifdef WORK_PINK
        num += seqPink;
        for (j = 0; i < num; i++, j++) {
            this->centers[i] = centerPink[j];
            this->colors[i] = PINK__;
        }
#endif
        this->numRect = num;  
#endif
    } catch (char* error) {
        std::cout << error << std::endl;
    }
}
